typedef float kal_t;

static void matvecmul_add_ge_6x2(kal_t *A, kal_t *x, kal_t *y, kal_t *z) {
	/*
	 x 2x1 vector
	 A 6x2 general
	 y 6x1 vector
	 z 6x1 vector 
	 z = z + y + Ax
	 */
    
	z[0] = z[0] + y[0] + A[0]*x[0] + A[6]*x[1];
	z[1] = z[1] + y[1] + A[1]*x[0] + A[7]*x[1];
	z[2] = z[2] + y[2] + A[2]*x[0] + A[8]*x[1];
	z[3] = z[3] + y[3] + A[3]*x[0] + A[9]*x[1];
	z[4] = z[4] + y[4] + A[4]*x[0] + A[10]*x[1];
	z[5] = z[5] + y[5] + A[5]*x[0] + A[11]*x[1];
}

static void matvecmul_tr_6x6_ip(kal_t *A, kal_t *x) {
	/*
	 x 6x1 vector
	 A 6x6 lower unit triangular 
	 x = Ax
	 */
	
	kal_t x1, x2, x3, x4, x5;
	x1 = x[1];
	x2 = x[2];
	x3 = x[3];
	x4 = x[4];
	x5 = x[5];
	
	x[1] = A[1]*x[0] + x1;
	x[2] = A[2]*x[0] + A[8]*x1  + x2;
	x[3] = A[3]*x[0] + A[9]*x1  + A[15]*x2 + x3;
	x[4] = A[4]*x[0] + A[10]*x1 + A[16]*x2 + A[22]*x3 + x4;
	x[5] = A[5]*x[0] + A[11]*x1 + A[17]*x2 + A[23]*x3 + A[29]*x4 + x5;
}

static void matvecmul_ge_6x6_ip(kal_t *A, kal_t *x) {
	/*
	 x 6x1 vector
	 A 6x6 general 
	 x = Ax
	 */
	
	kal_t x0, x1, x2, x3, x4, x5;
	x0 = x[0];
	x1 = x[1];
	x2 = x[2];
	x3 = x[3];
	x4 = x[4];
	x5 = x[5];
	
	x[0] = A[0]*x0 + A[6]*x1  + A[12]*x2 + A[18]*x3 + A[24]*x4 + A[30]*x5;
	x[1] = A[1]*x0 + A[7]*x1  + A[13]*x2 + A[19]*x3 + A[25]*x4 + A[31]*x5;
	x[2] = A[2]*x0 + A[8]*x1  + A[14]*x2 + A[20]*x3 + A[26]*x4 + A[32]*x5;
	x[3] = A[3]*x0 + A[9]*x1  + A[15]*x2 + A[21]*x3 + A[27]*x4 + A[33]*x5;
	x[4] = A[4]*x0 + A[10]*x1 + A[16]*x2 + A[22]*x3 + A[28]*x4 + A[34]*x5;
	x[5] = A[5]*x0 + A[11]*x1 + A[17]*x2 + A[23]*x3 + A[29]*x4 + A[35]*x5;

}

static void matsub_ge_6x6_ip(kal_t *a, kal_t *b) {
	/*
	 a = a - b
	 */
	a[0] = a[0] - b[0];
	a[1] = a[1] - b[1];
	a[2] = a[2] - b[2];
	a[3] = a[3] - b[3];
	a[4] = a[4] - b[4];
	a[5] = a[5] - b[5];
	
	a[6]  = a[6]  - b[6];
	a[7]  = a[7]  - b[7];
	a[8]  = a[8]  - b[8];
	a[9]  = a[9]  - b[9];
	a[10] = a[10] - b[10];
	a[11] = a[11] - b[11];
	
	a[12] = a[12] - b[12];
	a[13] = a[13] - b[13];
	a[14] = a[14] - b[14];
	a[15] = a[15] - b[15];
	a[16] = a[16] - b[16];
	a[17] = a[17] - b[17];
	
	a[18] = a[18] - b[18];
	a[19] = a[19] - b[19];
	a[20] = a[20] - b[20];
	a[21] = a[21] - b[21];
	a[22] = a[22] - b[22];
	a[23] = a[23] - b[23];
	
	a[24] = a[24] - b[24];
	a[25] = a[25] - b[25];
	a[26] = a[26] - b[26];
	a[27] = a[27] - b[27];
	a[28] = a[28] - b[28];
	a[29] = a[29] - b[29];
	
	a[30] = a[30] - b[30];
	a[31] = a[31] - b[31];
	a[32] = a[32] - b[32];
	a[33] = a[33] - b[33];
	a[34] = a[34] - b[34];
	a[35] = a[35] - b[35];	
}

static void matadd_sub_ge_6x6_ip(kal_t *a, kal_t *b, kal_t *c) {
	/*
	 a = a + b - c
	 */
	a[0] = a[0] + b[0] - c[0];
	a[1] = a[1] + b[1] - c[1];
	a[2] = a[2] + b[2] - c[2];
	a[3] = a[3] + b[3] - c[3];
	a[4] = a[4] + b[4] - c[4];
	a[5] = a[5] + b[5] - c[5];
	
	a[6]  = a[6]  + b[6]  - c[6];
	a[7]  = a[7]  + b[7]  - c[7];
	a[8]  = a[8]  + b[8]  - c[8];
	a[9]  = a[9]  + b[9]  - c[9];
	a[10] = a[10] + b[10] - c[10];
	a[11] = a[11] + b[11] - c[11];
	
	a[12] = a[12] + b[12] - c[12];
	a[13] = a[13] + b[13] - c[13];
	a[14] = a[14] + b[14] - c[14];
	a[15] = a[15] + b[15] - c[15];
	a[16] = a[16] + b[16] - c[16];
	a[17] = a[17] + b[17] - c[17];
	
	a[18] = a[18] + b[18] - c[18];
	a[19] = a[19] + b[19] - c[19];
	a[20] = a[20] + b[20] - c[20];
	a[21] = a[21] + b[21] - c[21];
	a[22] = a[22] + b[22] - c[22];
	a[23] = a[23] + b[23] - c[23];
	
	a[24] = a[24] + b[24] - c[24];
	a[25] = a[25] + b[25] - c[25];
	a[26] = a[26] + b[26] - c[26];
	a[27] = a[27] + b[27] - c[27];
	a[28] = a[28] + b[28] - c[28];
	a[29] = a[29] + b[29] - c[29];
	
	a[30] = a[30] + b[30] - c[30];
	a[31] = a[31] + b[31] - c[31];
	a[32] = a[32] + b[32] - c[32];
	a[33] = a[33] + b[33] - c[33];
	a[34] = a[34] + b[34] - c[34];
	a[35] = a[35] + b[35] - c[35];
}

static void matadd_ge_6x6(kal_t *a, kal_t *b, kal_t *c) {
	/*
	 c = a + b
	 */
	c[0] = a[0] + b[0];
	c[1] = a[1] + b[1];
	c[2] = a[2] + b[2];
	c[3] = a[3] + b[3];
	c[4] = a[4] + b[4];
	c[5] = a[5] + b[5];
	
	c[6]  = a[6]  + b[6];
	c[7]  = a[7]  + b[7];
	c[8]  = a[8]  + b[8];
	c[9]  = a[9]  + b[9];
	c[10] = a[10] + b[10];
	c[11] = a[11] + b[11];
	
	c[12] = a[12] + b[12];
	c[13] = a[13] + b[13];
	c[14] = a[14] + b[14];
	c[15] = a[15] + b[15];
	c[16] = a[16] + b[16];
	c[17] = a[17] + b[17];
	
	c[18] = a[18] + b[18];
	c[19] = a[19] + b[19];
	c[20] = a[20] + b[20];
	c[21] = a[21] + b[21];
	c[22] = a[22] + b[22];
	c[23] = a[23] + b[23];
	
	c[24] = a[24] + b[24];
	c[25] = a[25] + b[25];
	c[26] = a[26] + b[26];
	c[27] = a[27] + b[27];
	c[28] = a[28] + b[28];
	c[29] = a[29] + b[29];
	
	c[30] = a[30] + b[30];
	c[31] = a[31] + b[31];
	c[32] = a[32] + b[32];
	c[33] = a[33] + b[33];
	c[34] = a[34] + b[34];
	c[35] = a[35] + b[34];
	
}

static void matmul_ge_NN_6x6_ip(kal_t *a, kal_t *b) {
	/*
	 in place matrix multiply
	 b = a * b
	 a 6x6 general
	 b 6x6 general
	 */
	
	kal_t b0, b1, b2, b3, b4, b5;
	
	b0 = b[0];
	b1 = b[1];
	b2 = b[2];
	b3 = b[3];
	b4 = b[4];
	b5 = b[5];
	
	b[0] = a[0]*b0 + a[6]*b1  + a[12]*b2 + a[18]*b3 + a[24]*b4 + a[30]*b5;
	b[1] = a[1]*b0 + a[7]*b1  + a[13]*b2 + a[19]*b3 + a[25]*b4 + a[31]*b5;
	b[2] = a[2]*b0 + a[8]*b1  + a[14]*b2 + a[20]*b3 + a[26]*b4 + a[32]*b5;
	b[3] = a[3]*b0 + a[9]*b1  + a[15]*b2 + a[21]*b3 + a[27]*b4 + a[33]*b5;
	b[4] = a[4]*b0 + a[10]*b1 + a[16]*b2 + a[22]*b3 + a[28]*b4 + a[34]*b5;
	b[5] = a[5]*b0 + a[11]*b1 + a[17]*b2 + a[23]*b3 + a[29]*b4 + a[35]*b5;
	
	b0 = b[6];
	b1 = b[7];
	b2 = b[8];
	b3 = b[9];
	b4 = b[10];
	b5 = b[11];
	
	b[6]  = a[0]*b0 + a[6]*b1  + a[12]*b2 + a[18]*b3 + a[24]*b4 + a[30]*b5;
	b[7]  = a[1]*b0 + a[7]*b1  + a[13]*b2 + a[19]*b3 + a[25]*b4 + a[31]*b5;
	b[8]  = a[2]*b0 + a[8]*b1  + a[14]*b2 + a[20]*b3 + a[26]*b4 + a[32]*b5;
	b[9]  = a[3]*b0 + a[9]*b1  + a[15]*b2 + a[21]*b3 + a[27]*b4 + a[33]*b5;
	b[10] = a[4]*b0 + a[10]*b1 + a[16]*b2 + a[22]*b3 + a[28]*b4 + a[34]*b5;
	b[11] = a[5]*b0 + a[11]*b1 + a[17]*b2 + a[23]*b3 + a[29]*b4 + a[35]*b5;

	b0 = b[12];
	b1 = b[13];
	b2 = b[14];
	b3 = b[15];
	b4 = b[16];
	b5 = b[17];
	
	b[12] = a[0]*b0 + a[6]*b1  + a[12]*b2 + a[18]*b3 + a[24]*b4 + a[30]*b5;
	b[13] = a[1]*b0 + a[7]*b1  + a[13]*b2 + a[19]*b3 + a[25]*b4 + a[31]*b5;
	b[14] = a[2]*b0 + a[8]*b1  + a[14]*b2 + a[20]*b3 + a[26]*b4 + a[32]*b5;
	b[15] = a[3]*b0 + a[9]*b1  + a[15]*b2 + a[21]*b3 + a[27]*b4 + a[33]*b5;
	b[16] = a[4]*b0 + a[10]*b1 + a[16]*b2 + a[22]*b3 + a[28]*b4 + a[34]*b5;
	b[17] = a[5]*b0 + a[11]*b1 + a[17]*b2 + a[23]*b3 + a[29]*b4 + a[35]*b5;
	
	b0 = b[18];
	b1 = b[19];
	b2 = b[20];
	b3 = b[21];
	b4 = b[22];
	b5 = b[23];
	
	b[18] = a[0]*b0 + a[6]*b1  + a[12]*b2 + a[18]*b3 + a[24]*b4 + a[30]*b5;
	b[19] = a[1]*b0 + a[7]*b1  + a[13]*b2 + a[19]*b3 + a[25]*b4 + a[31]*b5;
	b[20] = a[2]*b0 + a[8]*b1  + a[14]*b2 + a[20]*b3 + a[26]*b4 + a[32]*b5;
	b[21] = a[3]*b0 + a[9]*b1  + a[15]*b2 + a[21]*b3 + a[27]*b4 + a[33]*b5;
	b[22] = a[4]*b0 + a[10]*b1 + a[16]*b2 + a[22]*b3 + a[28]*b4 + a[34]*b5;
	b[23] = a[5]*b0 + a[11]*b1 + a[17]*b2 + a[23]*b3 + a[29]*b4 + a[35]*b5;
	
	b0 = b[24];
	b1 = b[25];
	b2 = b[26];
	b3 = b[27];
	b4 = b[28];
	b5 = b[29];
	
	b[24] = a[0]*b0 + a[6]*b1  + a[12]*b2 + a[18]*b3 + a[24]*b4 + a[30]*b5;
	b[25] = a[1]*b0 + a[7]*b1  + a[13]*b2 + a[19]*b3 + a[25]*b4 + a[31]*b5;
	b[26] = a[2]*b0 + a[8]*b1  + a[14]*b2 + a[20]*b3 + a[26]*b4 + a[32]*b5;
	b[27] = a[3]*b0 + a[9]*b1  + a[15]*b2 + a[21]*b3 + a[27]*b4 + a[33]*b5;
	b[28] = a[4]*b0 + a[10]*b1 + a[16]*b2 + a[22]*b3 + a[28]*b4 + a[34]*b5;
	b[29] = a[5]*b0 + a[11]*b1 + a[17]*b2 + a[23]*b3 + a[29]*b4 + a[35]*b5;
	
	b0 = b[30];
	b1 = b[31];
	b2 = b[32];
	b3 = b[33];
	b4 = b[34];
	b5 = b[35];
	
	b[30] = a[0]*b0 + a[6]*b1  + a[12]*b2 + a[18]*b3 + a[24]*b4 + a[30]*b5;
	b[31] = a[1]*b0 + a[7]*b1  + a[13]*b2 + a[19]*b3 + a[25]*b4 + a[31]*b5;
	b[32] = a[2]*b0 + a[8]*b1  + a[14]*b2 + a[20]*b3 + a[26]*b4 + a[32]*b5;
	b[33] = a[3]*b0 + a[9]*b1  + a[15]*b2 + a[21]*b3 + a[27]*b4 + a[33]*b5;
	b[34] = a[4]*b0 + a[10]*b1 + a[16]*b2 + a[22]*b3 + a[28]*b4 + a[34]*b5;
	b[35] = a[5]*b0 + a[11]*b1 + a[17]*b2 + a[23]*b3 + a[29]*b4 + a[35]*b5;
}

static void matmul_tr_NN_6x6(kal_t *a, kal_t *b, kal_t *c) {
	/*
	 c = a * b
	 c 6x6 general
	 a 6x6 unit lower triangular
	 b 6x6 general
	 
	 used for A(PC') and A(PA')
	 */
	
	c[0] = b[0];
	c[1] = a[1]*b[0] + b[1];
	c[2] = a[2]*b[0] + a[8]*b[1]  + b[2];
	c[3] = a[3]*b[0] + a[9]*b[1]  + a[15]*b[2] + b[3];
	c[4] = a[4]*b[0] + a[10]*b[1] + a[16]*b[2] + a[22]*b[3] + b[4];
	c[5] = a[5]*b[0] + a[11]*b[1] + a[17]*b[2] + a[23]*b[3] + a[29]*b[4] + b[5];
	
	c[6]  = b[6];
	c[7]  = a[1]*b[6] + b[7];
	c[8]  = a[2]*b[6] + a[8]*b[7]  + b[8];
	c[9]  = a[3]*b[6] + a[9]*b[7]  + a[15]*b[8] + b[9];
	c[10] = a[4]*b[6] + a[10]*b[7] + a[16]*b[8] + a[22]*b[9] + b[10];
	c[11] = a[5]*b[6] + a[11]*b[7] + a[17]*b[8] + a[23]*b[9] + a[29]*b[10] + b[11];
	
	c[12] = b[12];
	c[13] = a[1]*b[12] + b[13];
	c[14] = a[2]*b[12] + a[8]*b[13]  + b[14];
	c[15] = a[3]*b[12] + a[9]*b[13]  + a[15]*b[14] + b[15];
	c[16] = a[4]*b[12] + a[10]*b[13] + a[16]*b[14] + a[22]*b[15] + b[16];
	c[17] = a[5]*b[12] + a[11]*b[13] + a[17]*b[14] + a[23]*b[15] + a[29]*b[16] + b[17];
	
	c[18] = b[18];
	c[19] = a[1]*b[18] + b[19];
	c[20] = a[2]*b[18] + a[8]*b[19]  + b[20];
	c[21] = a[3]*b[18] + a[9]*b[19]  + a[15]*b[20] + b[21];
	c[22] = a[4]*b[18] + a[10]*b[19] + a[16]*b[20] + a[22]*b[21] + b[22];
	c[23] = a[5]*b[18] + a[11]*b[19] + a[17]*b[20] + a[23]*b[21] + a[29]*b[22] + b[23];
	
	c[24] = b[24];
	c[25] = a[1]*b[24] + b[25];
	c[26] = a[2]*b[24] + a[8]*b[25]  + b[26];
	c[27] = a[3]*b[24] + a[9]*b[25]  + a[15]*b[26] + b[27];
	c[28] = a[4]*b[24] + a[10]*b[25] + a[16]*b[26] + a[22]*b[27] + b[28];
	c[29] = a[5]*b[24] + a[11]*b[25] + a[17]*b[26] + a[23]*b[27] + a[29]*b[28] + b[29];
	
	c[30] = b[30];
	c[31] = a[1]*b[30] + b[31];
	c[32] = a[2]*b[30] + a[8]*b[31]  + b[32];
	c[33] = a[3]*b[30] + a[9]*b[31]  + a[15]*b[32] + b[33];
	c[34] = a[4]*b[30] + a[10]*b[31] + a[16]*b[32] + a[22]*b[33] + b[34];
	c[35] = a[5]*b[30] + a[11]*b[31] + a[17]*b[32] + a[23]*b[33] + a[29]*b[34] + b[35];
}

static void matmul_tr_NT_6x6(kal_t *a, kal_t *b, kal_t *c) {
	/*
	 c = a * b
	 c 6x6 general
	 a 6x6 general
	 b 6x6 unit lower triangular - transpose
	 
	 used for PA'
	 */
	
	c[0] = a[0];
	c[1] = a[1];
	c[2] = a[2];
	c[3] = a[3];
	c[4] = a[4];
	c[5] = a[5];
	
	c[6]  = a[0]*b[1] + a[6];
	c[7]  = a[1]*b[1] + a[7];
	c[8]  = a[2]*b[1] + a[8];
	c[9]  = a[3]*b[1] + a[9];
	c[10] = a[4]*b[1] + a[10];
	c[11] = a[5]*b[1] + a[11];
	
	c[12] = a[0]*b[2] + a[6]*b[8]  + a[12];
	c[13] = a[1]*b[2] + a[7]*b[8]  + a[13];
	c[14] = a[2]*b[2] + a[8]*b[8]  + a[14];
	c[15] = a[3]*b[2] + a[9]*b[8]  + a[15];
	c[16] = a[4]*b[2] + a[10]*b[8] + a[16];
	c[17] = a[5]*b[2] + a[11]*b[8] + a[17];
	
	c[18] = a[0]*b[3] + a[6]*b[9]  + a[12]*b[15] + a[18];
	c[19] = a[1]*b[3] + a[7]*b[9]  + a[13]*b[15] + a[19];
	c[20] = a[2]*b[3] + a[8]*b[9]  + a[14]*b[15] + a[20];
	c[21] = a[3]*b[3] + a[9]*b[9]  + a[15]*b[15] + a[21];
	c[22] = a[4]*b[3] + a[10]*b[9] + a[16]*b[15] + a[22];
	c[23] = a[5]*b[3] + a[11]*b[9] + a[17]*b[15] + a[23];
	
	c[24] = a[0]*b[4] + a[6]*b[10]  + a[12]*b[16] + a[18]*b[22] + a[24];
	c[25] = a[1]*b[4] + a[7]*b[10]  + a[13]*b[16] + a[19]*b[22] + a[25];
	c[26] = a[2]*b[4] + a[8]*b[10]  + a[14]*b[16] + a[20]*b[22] + a[26];
	c[27] = a[3]*b[4] + a[9]*b[10]  + a[15]*b[16] + a[21]*b[22] + a[27];
	c[28] = a[4]*b[4] + a[10]*b[10] + a[16]*b[16] + a[22]*b[22] + a[28];
	c[29] = a[5]*b[4] + a[11]*b[10] + a[17]*b[16] + a[23]*b[22] + a[29];
	
	c[30] = a[0]*b[5] + a[6]*b[11]  + a[12]*b[17] + a[18]*b[23] + a[24]*b[29] + a[30];
	c[31] = a[1]*b[5] + a[7]*b[11]  + a[13]*b[17] + a[19]*b[23] + a[25]*b[29] + a[31];
	c[32] = a[2]*b[5] + a[8]*b[11]  + a[14]*b[17] + a[20]*b[23] + a[26]*b[29] + a[32];
	c[33] = a[3]*b[5] + a[9]*b[11]  + a[15]*b[17] + a[21]*b[23] + a[27]*b[29] + a[33];
	c[34] = a[4]*b[5] + a[10]*b[11] + a[16]*b[17] + a[22]*b[23] + a[28]*b[29] + a[34];
	c[35] = a[5]*b[5] + a[11]*b[11] + a[17]*b[17] + a[23]*b[23] + a[29]*b[29] + a[35];
}

static void solve(kal_t *A, kal_t *B) {
	/*
	 A 6x6 general matrix, input (APC')/output(K)
	 B 6x6 general matrix, input (CPC'+Sz) - destroyed
	 
	 solve K = APC'(CPC'+Sz)^(-1) for K.
	 rewrite as
	 A = APC' and B = (CPC'+Sz)
	 K = AB^(-1)
	 then as
	 KB = A
	 
	 1) perform gaussian elimination with column pivoting on B 
	 leaving lower triangular
	 
	 2) back solve for K, inplace A

	 -----------------------
	 to check, at end K == AB^(-1)
	*/
	
	
	int i, j, k;
	
	// start factorization
	for (i = 0; i < 6; ++i) {
		kal_t *currLoc = B + i*6 + i;
		
		// determine if pivot is necessary
		int piv = i;
		kal_t pivMax = currLoc[0];
		currLoc += 6;
		for (j = i+1; j < 6; ++j) {
			if (currLoc[0] > pivMax) {
				piv = j;
				pivMax = currLoc[0];
			}
			currLoc += 6;
		}
		
		// pivot when appropriate
		if (i != piv) {
			kal_t *Ac0 = A + i*6;
			kal_t *Ac1 = A + piv*6;
			
			kal_t *Bc0 = B + i*6;
			kal_t *Bc1 = B + piv*6;
			
			for (j = 0; j < 6; ++j) {
				kal_t tmp = Ac0[j];
				Ac0[j] = Ac1[j];
				Ac1[j] = tmp;
				
				tmp = Bc0[j];
				Bc0[j] = Bc1[j];
				Bc1[j] = tmp;
			}
		}
		
		// calculate scales and store inplace
		kal_t *currCol = B + i*6;
		for (j = i+1; j < 6; ++j) {
			currCol[j] = currCol[j]/currCol[i];
		}
		
		// update
		kal_t *fact = B + i*6;
		for (j = i+1; j < 6; ++j) {
			currCol = B + j*6;
			for (k = i+1; k < 6; ++k) {
				currCol[k] -= currCol[i]*fact[k];
			}
		}
		
#ifdef DEBUG
		// DEBUG only
		// zero lower triangular
		currCol = B + i*6;
		for (j = i+1; j < 6; ++j) {
			currCol[j] = 0;
		}
#endif
	}
	// end factorization
	
	
	// perform back solve for each row
	for (k = 0; k < 6; ++k) {
		kal_t *Br0 = B + k;
		
		for (i = 0; i < 6; ++i) {
			kal_t *Ac0 = A + i*6;
			kal_t acc0 = 0.0;
			
			for (j = 0; j <= i; ++j) {
				acc0 += Br0[j*6] * Ac0[j];
			}
			Br0[i*6] = (Br0[i*6] - acc0) / Ac0[i];
		}
	}
}

static void matinverse_6x6(kal_t *a) {
	/*
	 in place 6x6 inverse
	 inverse of 6x6 matrix
	 | a11 a12 a13 |-1             |   a33a22-a32a23  -(a33a12-a32a13)   a23a12-a22a13  |
	 | a21 a22 a23 |    =  1/DET * | -(a33a21-a31a23)   a33a11-a31a13  -(a23a11-a21a13) |
	 | a31 a32 a33 |               |   a32a21-a31a22  -(a32a11-a31a12)   a22a11-a21a12  |
	 
	 with DET  =  a11(a33a22-a32a23)-a21(a33a12-a32a13)+a31(a23a12-a22a13)
	 */
	// column major 6x6 matrices
#define a11 a[0]
#define a21 a[1]
#define a31 a[2]
	
#define a12 a[3]
#define a22 a[4]
#define a32 a[5]
	
#define a13 a[6]
#define a23 a[7]
#define a33 a[8]
	
	kal_t ta11, ta12, ta13;
	ta11 = a33*a22-a32*a23;
	ta12 = a33*a12-a32*a13;
	ta13 = a23*a12-a22*a13;
	kal_t det = a11*ta11-a21*ta12+a31*ta13;
	det = 1/det;
	
	// put top row in place
	kal_t tmp = a11;
	a11 = ta11*det;
	ta11 = tmp;
	
	tmp = a12;
	a12 = -ta12*det;
	ta12 = tmp;
	
	tmp = a13;
	a13 = ta13*det;
	ta13 = tmp;
	
	// have bottom 2x3 to complete
	tmp = a23;
	a23 = -(a23*ta11-a21*ta13)*det;
	
	kal_t tmp1 = a21;
	a21 = -(a33*a21-a31*tmp)*det;
	
	tmp = a22;
	a22 = (a33*ta11-a31*ta13)*det;
	
	a33 = (tmp*ta11-tmp1*ta12)*det;
	
	tmp = (a32*tmp1-a31*tmp)*det;
	a32 = -(a32*ta11-a31*ta12)*det;
	a31 = tmp;  
}


void kalman() {
	// time dependent
	kal_t A[36];      // 6x6 relationship between state variables
	kal_t B[12];      // 6x2 relationsiop between commanded input and state variables
	
	// constants  
	//kal_t C[36];    // relationship between sensor measurements and expected state variables
	// C is currently identity and not needed
	kal_t Sz[36];     // 6x6 noise caused by instrument error 
	//    (large values means do not give much credibility to measurements)
	kal_t Szi[36];    // 6x6 inverse of Sz
	kal_t Sw[36];     // 6x6 process noise
	
	// input and state information
	kal_t P[36];      // 6x6 error covariance
	kal_t K[36];      // 6x6 kalman gain
	kal_t x[6];		  // state information {acceleration_x, acceleration_y, 
					  // velocity_x, velocity_y, position_x, position_y}
	kal_t y[6];       // sensor input {acceleration_x, acceleration_y, 
					  // velocity_x, velocity_y, position_x, position_y}
	kal_t u[2];       // commanded input {acceleration_x, acceleration_y}
	
	// temporary data structures
	kal_t tmp[36];
	kal_t *PCt;
	kal_t PAt[36];
	
	// compute PAt and PCt
	matmul_tr_NT_6x6(P,A,PAt);
	// C is currently identity matrix so PCt = P
	// done with this iterations use of P so just
	// pointing PCt to P
	PCt = P;
	
	// kalman gain
	// K = APC'(CPC'+Sz)^-1
	matmul_tr_NN_6x6(A,PCt,tmp);
	// C is currently identity 
	matadd_ge_6x6(Sz,PCt,K);
	//matinverse_6x6(K);
  solve(K,tmp);
	matmul_ge_NN_6x6_ip(tmp,K);
	
	// next state
	//x = (Ax + Bu) + K(y-Cx)
	// C is currently identity matrix
	matsub_ge_6x6_ip(y,x);
	matvecmul_ge_6x6_ip(K,y);
	matvecmul_tr_6x6_ip(A,x);
	matvecmul_add_ge_6x2(B,u,y,x);
	
	// calculate error covariance
	// NOTE: PCt points to P
	matmul_tr_NN_6x6(A,PCt,tmp);
	// C is currently identity
	matmul_tr_NN_6x6(A,PAt,P);
	matmul_ge_NN_6x6_ip(Szi,PAt);
	matmul_ge_NN_6x6_ip(tmp,PAt);
	matadd_sub_ge_6x6_ip(P,Sw,PAt);
}



/*
 K = AP inv(P + S)
 rewrites to
 K(P+S) = AP
 
 do gaussian elimination using columns of (P+S)
 
 for one row of unknow K, (P+S) will be lower triangular
 and will have update one row of AP
			 s 0 0 0 0 0
			 s s 0 0 0 0
			 s s s 0 0 0
			 s s s s 0 0
			 s s s s s 0
			 s s s s s s
 
 k k k k k k a a a a a a
 
 allowing for backsolve of the current K row.
 do this backsolve for each k row.

*/

